#include <stdint.h>
#include <stdio.h>

#include "config.h"
#include "core.h"
#include "reg.h"
#include "../bus/xbar.h"

#define OPCODE_LUI      0x37
#define OPCODE_AUIPC    0x17
#define OPCODE_JAL      0x6f
#define OPCODE_JALR     0x67
#define OPCODE_BXX      0x63
#define OPCODE_LOAD     0x03
#define OPCODE_STORE    0x23
#define OPCODE_IMM      0x13
#define OPCODE_ALU      0x33
#define OPCODE_FENCE    0x0f
#define OPCODE_SYSTEM   0x73

#define BEQ             0x0
#define BNE             0x1
#define BLT             0x4
#define BGE             0x5
#define BLTU            0x6
#define BGEU            0x7

#define LB              0x0
#define LH              0x1
#define LW              0x2
#define LBU             0x4
#define LHU             0x5

#define SB              0x0
#define SH              0x1
#define SW              0x2

#define ADDI            0x0
#define SLTI            0x2
#define SLTIU           0x3
#define XORI            0x4
#define ORI             0x6
#define ANDI            0x7
#define SLLI            0x1
#define SRI             0x5

#define ADD_SUB         0x0
#define SLL             0x1
#define SLT             0x2
#define SLTU            0x3
#define XOR             0x4
#define SR              0x5
#define OR              0x6
#define AND             0x7

#define MUL             0x0
#define MULH            0x1
#define MULHSU          0x2
#define MULHU           0x3
#define DIV             0x4
#define DIVU            0x5
#define REM             0x6
#define REMU            0x7

#define CSRRW           0x1
#define CSRRS           0x2
#define CSRRC           0x3
#define CSRRWI          0x5
#define CSRRSI          0x6
#define CSRRCI          0x7


extern uint32_t core_get_pc();
extern void core_set_pc(uint32_t val);


static int32_t u_type_imm(uint32_t instr)
{
    int32_t imm;

    imm = instr & (~0xfff);

    return imm;
}

static int32_t j_type_imm(uint32_t instr)
{
    int32_t imm;

    if ((instr >> 31) & 0x1)
        imm = 0xfff00000;
    else
        imm = 0x0;
    imm |= ((instr >> 12) & 0xff) << 12;
    imm |= ((instr >> 20) & 0x1) << 11;
    imm |= ((instr >> 21) & 0x3ff) << 1;

    return imm;
}

static int32_t b_type_imm(uint32_t instr)
{
    int32_t imm;

    if ((instr >> 31) & 0x1)
        imm = 0xfffff000;
    else
        imm = 0x0;
    imm |= ((instr >> 7) & 0x1) << 11;
    imm |= ((instr >> 25) & 0x3f) << 5;
    imm |= ((instr >> 8) & 0xf) << 1;

    return imm;
}

static int32_t s_type_imm(uint32_t instr)
{
    int32_t imm;

    if ((instr >> 31) & 0x1)
        imm = 0xfffff000;
    else
        imm = 0x0;
    imm |= ((instr >> 25) & 0x7f) << 5;
    imm |= ((instr >> 7) & 0x1f) << 0;

    return imm;
}

static int32_t i_type_imm(uint32_t instr)
{
    int32_t imm;

    if ((instr >> 31) & 0x1)
        imm = 0xfffff000;
    else
        imm = 0x0;
    imm |= ((instr >> 20) & 0xfff) << 0;

    return imm;
}

void instr_decode_and_exec(uint32_t instr)
{
    uint32_t opcode;
    uint32_t rs1, rs2, rd;
    int32_t imm;
    uint32_t funct3, funct7;
    uint32_t op_res;
    uint32_t rs1_data;
    uint32_t rs2_data;
    uint32_t pc;
    uint32_t mem_addr;
    uint32_t mem_data;
    uint32_t csr;
    uint32_t tmp32;
    uint64_t tmp64;

    opcode = instr & 0x7f;
    rs1 = (instr >> 15) & 0x1f;
    rs2 = (instr >> 20) & 0x1f;
    rd = (instr >> 7) & 0x1f;
    funct3 = (instr >> 12) & 0x7;
    funct7 = (instr >> 25) & 0x7f;

    csr = (instr >> 20) & 0xfff;
    rs1_data = reg_gpr_get(rs1);
    rs2_data = reg_gpr_get(rs2);
    pc = core_get_pc();

    core_set_pc(pc + 4);

    #if (DEBUG_EN == 1)
    printf("%s() start exec, pc: 0x%08x, instr: 0x%08x, opcode: 0x%x\n", __func__, pc, instr, opcode);
    #endif

    switch (opcode) {
        case OPCODE_LUI:
            imm = u_type_imm(instr);
            reg_gpr_set(rd, imm);
            break;

        case OPCODE_AUIPC:
            imm = u_type_imm(instr);
            op_res = pc + imm;
            reg_gpr_set(rd, op_res);
            break;

        case OPCODE_JAL:
            imm = j_type_imm(instr);
            op_res = pc + imm;
            reg_gpr_set(rd, pc + 4);
            core_set_pc(op_res);
            break;

        case OPCODE_JALR:
            imm = i_type_imm(instr);
            op_res = rs1_data + imm;
            reg_gpr_set(rd, pc + 4);
            core_set_pc(op_res);
            break;

        case OPCODE_BXX:
            imm = b_type_imm(instr);
            op_res = pc + imm;
            switch (funct3) {
                case BEQ:
                    if (rs1_data == rs2_data)
                        core_set_pc(op_res);
                    break;
                case BNE:
                    if (rs1_data != rs2_data)
                        core_set_pc(op_res);
                    break;
                case BLT:
                    if (((int32_t)rs1_data) < ((int32_t)rs2_data))
                        core_set_pc(op_res);
                    break;
                case BGE:
                    if (((int32_t)rs1_data) >= ((int32_t)rs2_data))
                        core_set_pc(op_res);
                    break;
                case BLTU:
                    if (rs1_data < rs2_data)
                        core_set_pc(op_res);
                    break;
                case BGEU:
                    if (rs1_data >= rs2_data)
                        core_set_pc(op_res);
                    break;
                default:
                    break;
            }
            break;

        case OPCODE_LOAD:
            imm = i_type_imm(instr);
            mem_addr = rs1_data + imm;
            mem_data = xbar_master_read(mem_addr);
            switch (funct3) {
                case LB:
                    if (mem_data & BIT7)
                        op_res = 0xffffff00 | (mem_data & 0xff);
                    else
                        op_res = mem_data & 0xff;
                    reg_gpr_set(rd, op_res);
                    break;
                case LH:
                    if (mem_data & BIT15)
                        op_res = 0xffff0000 | (mem_data & 0xffff);
                    else
                        op_res = mem_data & 0xffff;
                    reg_gpr_set(rd, op_res);
                    break;
                case LW:
                    op_res = mem_data;
                    reg_gpr_set(rd, op_res);
                    break;
                case LBU:
                    op_res = mem_data & 0xff;
                    reg_gpr_set(rd, op_res);
                    break;
                case LHU:
                    op_res = mem_data & 0xffff;
                    reg_gpr_set(rd, op_res);
                    break;
                default:
                    break;
            }
            break;

        case OPCODE_STORE:
            imm = s_type_imm(instr);
            mem_addr = rs1_data + imm;
            switch (funct3) {
                case SB:
                    op_res = rs2_data & 0xff;
                    xbar_master_write(mem_addr, 0x1, op_res);
                    break;
                case SH:
                    op_res = rs2_data & 0xffff;
                    xbar_master_write(mem_addr, 0x3, op_res);
                    break;
                case SW:
                    op_res = rs2_data;
                    xbar_master_write(mem_addr, 0xf, op_res);
                    break;
                default:
                    break;
            }
            break;

        case OPCODE_IMM:
            imm = i_type_imm(instr);
            switch (funct3) {
                case ADDI:
                    op_res = rs1_data + imm;
                    reg_gpr_set(rd, op_res);
                    break;
                case SLTI:
                    if (((int32_t)rs1_data) < ((int32_t)imm))
                        op_res = 0x1;
                    else
                        op_res = 0x0;
                    reg_gpr_set(rd, op_res);
                    break;
                case SLTIU:
                    if (rs1_data < imm)
                        op_res = 0x1;
                    else
                        op_res = 0x0;
                    reg_gpr_set(rd, op_res);
                    break;
                case XORI:
                    op_res = rs1_data ^ imm;
                    reg_gpr_set(rd, op_res);
                    break;
                case ORI:
                    op_res = rs1_data | imm;
                    reg_gpr_set(rd, op_res);
                    break;
                case ANDI:
                    op_res = rs1_data & imm;
                    reg_gpr_set(rd, op_res);
                    break;
                case SLLI:
                    op_res = rs1_data << ((instr >> 20) & 0x1f);
                    reg_gpr_set(rd, op_res);
                    break;
                case SRI:
                    // SRLI
                    if (funct7 == 0x0) {
                        op_res = rs1_data >> ((instr >> 20) & 0x1f);
                    // SRAI
                    } else {
                        if ((rs1_data >> 31) & 0x1)
                            op_res = (rs1_data >> ((instr >> 20) & 0x1f)) |
                                       (~(0xffffffff >> ((instr >> 20) & 0x1f)));
                        else
                            op_res = rs1_data >> ((instr >> 20) & 0x1f);
                    }
                    reg_gpr_set(rd, op_res);
                    break;
                default:
                    break;
            }
            break;

        case OPCODE_ALU:
            // MUL/DIV
            if (funct7 == 0x1) {
                switch (funct3) {
                    case MUL:
                        tmp64 = (uint64_t)rs1_data * (uint64_t)rs2_data;
                        op_res = tmp64 & 0xffffffff;
                        reg_gpr_set(rd, op_res);
                        break;
                    case MULH:
                        if (rs1_data & BIT31)
                            tmp64 = ~rs1_data + 1;
                        else
                            tmp64 = rs1_data;
                        if (rs2_data & BIT31)
                            tmp64 = tmp64 * (~rs2_data + 1);
                        else
                            tmp64 = tmp64 * rs2_data;
                        if ((rs1_data & BIT31) ^ (rs2_data & BIT31))
                            tmp64 = ~tmp64 + 1;
                        op_res = (tmp64 >> 32) & 0xffffffff;
                        reg_gpr_set(rd, op_res);
                        break;
                    case MULHU:
                        tmp64 = (uint64_t)rs1_data * (uint64_t)rs2_data;
                        op_res = tmp64 >> 32;
                        reg_gpr_set(rd, op_res);
                        break;
                    case MULHSU:
                        if (rs1_data & BIT31)
                            tmp64 = ~rs1_data + 1;
                        else
                            tmp64 = rs1_data;
                        tmp64 = tmp64 * rs2_data;
                        if (rs1_data & BIT31)
                            tmp64 = ~tmp64 + 1;
                        op_res = (tmp64 >> 32) & 0xffffffff;
                        reg_gpr_set(rd, op_res);
                        break;
                    case DIV:
                        if (rs2_data == 0) {
                            op_res = 0xffffffff;
                        } else {
                            if (rs1_data & BIT31)
                                op_res = ~rs1_data + 1;
                            else
                                op_res = rs1_data;
                            if (rs2_data & BIT31)
                                op_res = op_res / (~rs2_data + 1);
                            else
                                op_res = op_res / rs2_data;
                            if ((rs1_data & BIT31) ^ (rs2_data & BIT31))
                                op_res = ~op_res + 1;
                        }
                        reg_gpr_set(rd, op_res);
                        break;
                    case DIVU:
                        if (rs2_data == 0) {
                            op_res = 0xffffffff;
                        } else {
                            op_res = rs1_data / rs2_data;
                        }
                        reg_gpr_set(rd, op_res);
                        break;
                    case REM:
                        if (rs2_data == 0) {
                            op_res = rs1_data;
                        } else {
                            if (rs1_data & BIT31)
                                op_res = ~rs1_data + 1;
                            else
                                op_res = rs1_data;
                            if (rs2_data & BIT31)
                                op_res = op_res % (~rs2_data + 1);
                            else
                                op_res = op_res % rs2_data;
                            if (rs1_data & BIT31)
                                op_res = ~op_res + 1;
                        }
                        reg_gpr_set(rd, op_res);
                        break;
                    case REMU:
                        if (rs2_data == 0) {
                            op_res = rs1_data;
                        } else {
                            op_res = rs1_data % rs2_data;
                        }
                        reg_gpr_set(rd, op_res);
                        break;
                    default:
                        break;
                }
            } else {
                switch (funct3) {
                    case ADD_SUB:
                        // ADD
                        if (funct7 == 0x0) {
                            op_res = rs1_data + rs2_data;
                        // SUB
                        } else {
                            op_res = rs1_data - rs2_data;
                        }
                        reg_gpr_set(rd, op_res);
                        break;
                    case SLL:
                        op_res = rs1_data << (rs2_data & 0x1f);
                        reg_gpr_set(rd, op_res);
                        break;
                    case SLT:
                        op_res = (((int32_t)rs1_data) < ((int32_t)rs2_data));
                        reg_gpr_set(rd, op_res);
                        break;
                    case SLTU:
                        if (rs1_data < rs2_data)
                            op_res = 0x1;
                        else
                            op_res = 0x0;
                        reg_gpr_set(rd, op_res);
                        break;
                    case XOR:
                        op_res = rs1_data ^ rs2_data;
                        reg_gpr_set(rd, op_res);
                        break;
                    case SR:
                        // SRL
                        if (funct7 == 0x0) {
                            op_res = rs1_data >> (rs2_data & 0x1f);
                        // SRA
                        } else {
                            if ((rs1_data >> 31) & 0x1)
                                op_res = (rs1_data >> (rs2_data & 0x1f)) |
                                           (~(0xffffffff >> (rs2_data & 0x1f)));
                            else
                                op_res = rs1_data >> (rs2_data & 0x1f);
                        }
                        reg_gpr_set(rd, op_res);
                        break;
                    case OR:
                        op_res = rs1_data | rs2_data;
                        reg_gpr_set(rd, op_res);
                        break;
                    case AND:
                        op_res = rs1_data & rs2_data;
                        reg_gpr_set(rd, op_res);
                        break;
                    default:
                        break;
                }
            }
            break;

        case OPCODE_FENCE:
            // nop
            break;

        case OPCODE_SYSTEM:
            switch (funct3) {
                case 0x0:
                    // EBREAK
                    if (instr & BIT20) {
                        
                    // ECALL
                    } else {
                        
                    }
                    break;
                case CSRRW:
                    tmp32 = reg_csr_get(csr);
                    reg_csr_set(csr, rs1_data);
                    reg_gpr_set(rd, tmp32);
                    break;
                case CSRRS:
                    tmp32 = reg_csr_get(csr);
                    reg_csr_set(csr, tmp32 | rs1_data);
                    reg_gpr_set(rd, tmp32);
                    break;
                case CSRRC:
                    tmp32 = reg_csr_get(csr);
                    reg_csr_set(csr, tmp32 & (~rs1_data));
                    reg_gpr_set(rd, tmp32);
                    break;
                case CSRRWI:
                    tmp32 = reg_csr_get(csr);
                    reg_csr_set(csr, rs1);
                    reg_gpr_set(rd, tmp32);
                    break;
                case CSRRSI:
                    tmp32 = reg_csr_get(csr);
                    reg_csr_set(csr, tmp32 | rs1);
                    reg_gpr_set(rd, tmp32);
                    break;
                case CSRRCI:
                    tmp32 = reg_csr_get(csr);
                    reg_csr_set(csr, tmp32 & (~rs1));
                    reg_gpr_set(rd, tmp32);
                    break;
                default:
                    break;
            }
            break;

        default:
            break;
    }

}
